#include "CollisionDetector.h"

using namespace PHYOgre;

CollisionDetector::CollisionDetector(Ogre::SceneManager * iSceneManager)
	:mSceneManager(iSceneManager)
	//,mCurrCollisionNum(0)
{
	RegisterCollisionMethod(std::make_pair(SPHERE,PLANE),&CollisionDetector::HandleCollisionSpherePlane);
	RegisterCollisionMethod(std::make_pair(PLANE,SPHERE),&HandleCollisionPlaneSphere);
	RegisterCollisionMethod(std::make_pair(SPHERE,SPHERE),&HandleCollisionSphereSphere);
	RegisterCollisionMethod(std::make_pair(BOX,BOX),&HandleCollisionBoxBox);
	RegisterCollisionMethod(std::make_pair(BOX,PLANE),&HandleCollisionBoxPlane);
	RegisterCollisionMethod(std::make_pair(PLANE,BOX),&HandleCollisionPlaneBox);
}

void CollisionDetector::RegisterCollisionMethod(ShapePair iShapes,CollisionMethod iMethod)
{
	mCollisionMethods[iShapes] = iMethod;
}
CollisionDetector::CollisionDetector(void)
{
}

CollisionDetector::~CollisionDetector(void)
{
}
void CollisionDetector::SetSceneManager(Ogre::SceneManager * iSceneManager)
{
	mSceneManager = iSceneManager;
}
void CollisionDetector::GenerateCollisions()
{
	Ogre::IntersectionSceneQuery* iQuery =mSceneManager->createIntersectionQuery();
	iQuery->setQueryMask(MASK_RIGID_BODY | MASK_PARTICLE | MASK_PLANE);
	Ogre::IntersectionSceneQueryResult& queryResult = iQuery->execute();

	Ogre::SceneQueryMovableIntersectionList::iterator end = queryResult.movables2movables.end();
	for ( Ogre::SceneQueryMovableIntersectionList::iterator it = queryResult.movables2movables.begin(); it != end; ++it)
	{
		Ogre::String name = it->first->getName();

		if (Ogre::any_cast<IntegrableInterface*>(&it->first->getUserAny()))
		{
			IntegrableInterface *iface = Ogre::any_cast<IntegrableInterface*>(it->first->getUserAny());
			IntegrableInterface *iface2 = Ogre::any_cast<IntegrableInterface*>(it->second->getUserAny());
			
			ShapePair collisionPair = std::make_pair<ShapeType,ShapeType >(iface->GetShape()->GetType(),iface2->GetShape()->GetType());

			if (mCollisionMethods[collisionPair])
				mCollisionMethods[collisionPair](iface->GetShape(),iface2->GetShape(), iface, iface2,&mCollisionData);
		}
	}
	Ogre::SceneQueryMovableWorldFragmentIntersectionList::iterator end2 = queryResult.movables2world.end();
	for ( Ogre::SceneQueryMovableWorldFragmentIntersectionList::iterator it = queryResult.movables2world.begin(); it != end2; ++it)
	{
		Ogre::String name = it->first->getName();
	}
	mSceneManager->destroyQuery(iQuery);
}




void CollisionDetector::HandleCollisionSpherePlane(const Shape* iSphere, const Shape* iPlane,const  IntegrableInterface* obj1,const IntegrableInterface* obj2, CollisionData *iContact)
{
		
}
void CollisionDetector::HandleCollisionPlaneSphere(const Shape* iPlane, const Shape* iSphere,const  IntegrableInterface* obj1,const IntegrableInterface* obj2, CollisionData *iContact)
{

}
void CollisionDetector::HandleCollisionSphereSphere(const Shape* iSphere1, const Shape* iSphere2,const  IntegrableInterface* obj1,const IntegrableInterface* obj2, CollisionData *iContact)
{

}
void CollisionDetector::HandleCollisionBoxBox(const Shape* iBox1, const Shape* iBox2,const  IntegrableInterface* obj1,const IntegrableInterface* obj2, CollisionData *iContact)
{

}
void CollisionDetector::HandleCollisionBoxPlane(const Shape* iBox, const Shape* iPlane,const  IntegrableInterface* obj1,const IntegrableInterface* obj2, CollisionData *iContact)
{
	std::cout << "BOX PLANE" <<"\n";

	const Box *box = static_cast<const Box*>(iBox);
	const Plane *plane = static_cast<const Plane*>(iPlane);

	for (int i = 0 ; i < 8; ++i)
	{
		if (!iContact->HasMoreSpace())
			return;
		Vector3 foo = box->mVertices[i];
		Real vertexDistance = box->mVertices[i].dotProduct(plane->mNormal);

		Contact *contactPtr = &iContact->mContactListPtr[iContact->mLength];
		if (vertexDistance <= plane->mOffset)
		{
			contactPtr->mBodies[0] = obj1;
			contactPtr->mBodies[1] = obj2;
			contactPtr->mPoint = plane->mNormal;
			contactPtr->mPoint *= vertexDistance - plane->mOffset;
			contactPtr->mPoint = box->mVertices[i];
			contactPtr->mNormal = plane->mNormal;
			contactPtr->mPenetration = plane->mOffset - vertexDistance;
		}
		iContact->mLength +=1;
	}
	

}
void CollisionDetector::HandleCollisionPlaneBox(const Shape* iPlane, const Shape* iBox, const  IntegrableInterface* obj1,const IntegrableInterface* obj2,CollisionData *iContact)
{
	HandleCollisionBoxPlane(iBox,iPlane,obj1,obj2,iContact);
}